#include <cmath>
#include <robot_chassis/CTurnOnRobot.hpp>
#include <cstddef>
#include <cstdint>
#include <thread>
#include <chrono>
#include <iostream>
#include <cstdio>
#include <sys/stat.h>
#include <unistd.h>
#include <errno.h>
#include <cstring>

// 简单的日志宏，用于替换 tools 包的 LOG_INFO 和 LOG_ERROR
#define LOG_INFO(fmt, ...) \
    do { \
        std::printf("[INFO] " fmt "\n", ##__VA_ARGS__); \
    } while(0)

#define LOG_ERROR(fmt, ...) \
    do { \
        std::fprintf(stderr, "[ERROR] " fmt "\n", ##__VA_ARGS__); \
    } while(0)

CTurnOnRobot& CTurnOnRobot::getInstance()
{
    static CTurnOnRobot instance;
    return instance;
}

CTurnOnRobot::CTurnOnRobot() 
    : io_context_(std::make_shared<drivers::common::IoContext>())
{
}

CTurnOnRobot::~CTurnOnRobot()
{
    isRunning_ = false;
    closeUsart();
}

bool CTurnOnRobot::openUsart(const SUsartParameters& usartParameters)
{
    // 检查串口设备文件是否存在
    struct stat fileStat;
    if (stat(usartParameters.usartPortName_.c_str(), &fileStat) != 0) {
        if (errno == ENOENT) {
            LOG_ERROR("串口设备不存在: %s", usartParameters.usartPortName_.c_str());
            LOG_ERROR("请检查:");
            LOG_ERROR("  1. 设备是否已连接");
            LOG_ERROR("  2. 串口路径是否正确 (常见路径: /dev/ttyACM0, /dev/ttyACM0)");
            LOG_ERROR("  3. 可以使用 'ls -l /dev/ttyACM* /dev/ttyACM*' 查看可用串口");
        } else {
            LOG_ERROR("无法访问串口设备 %s: %s", 
                      usartParameters.usartPortName_.c_str(), 
                      strerror(errno));
        }
        return false;
    }

    // 检查是否为字符设备
    if (!S_ISCHR(fileStat.st_mode)) {
        LOG_ERROR("指定的路径不是字符设备: %s", usartParameters.usartPortName_.c_str());
        return false;
    }

    // 检查是否有读取权限
    if (access(usartParameters.usartPortName_.c_str(), R_OK | W_OK) != 0) {
        LOG_ERROR("没有权限访问串口设备: %s", usartParameters.usartPortName_.c_str());
        LOG_ERROR("请尝试:");
        LOG_ERROR("  1. 将当前用户添加到 dialout 组: sudo usermod -a -G dialout $USER");
        LOG_ERROR("  2. 或者使用 sudo 运行程序");
        LOG_ERROR("  3. 或者修改设备权限: sudo chmod 666 %s", usartParameters.usartPortName_.c_str());
        return false;
    }

    try {
        // 配置串口参数
        drivers::serial_driver::SerialPortConfig config(
            usartParameters.usartBaudRate_,
            drivers::serial_driver::FlowControl::NONE,
            drivers::serial_driver::Parity::NONE,
            drivers::serial_driver::StopBits::ONE
        );

        // 创建串口对象
        mcuSerial_ = std::make_shared<drivers::serial_driver::SerialPort>(
            *io_context_,
            usartParameters.usartPortName_,
            config
        );

        // 打开串口
        mcuSerial_->open();
        
        LOG_INFO("串口 %s 打开成功！波特率: %d", 
                 usartParameters.usartPortName_.c_str(), 
                 usartParameters.usartBaudRate_);
        return true;
    } catch (const std::exception& e) {
        LOG_ERROR("串口 %s 打开失败: %s", 
                  usartParameters.usartPortName_.c_str(), e.what());
        LOG_ERROR("可能的原因:");
        LOG_ERROR("  1. 串口被其他程序占用");
        LOG_ERROR("  2. 波特率配置不正确");
        LOG_ERROR("  3. 设备驱动问题");
        LOG_ERROR("提示: 可以使用 'lsof %s' 或 'fuser %s' 检查是否有其他程序占用", 
                  usartParameters.usartPortName_.c_str(),
                  usartParameters.usartPortName_.c_str());
        return false;
    }
}

void CTurnOnRobot::startReceive()
{
    isRunning_ = true;
    std::thread t(&CTurnOnRobot::loopReceive, this);
    t.detach();
    return;
}

void CTurnOnRobot::loopReceive()
{
    LOG_INFO("串口接收线程已启动，开始监听数据...");
    int loopCount = 0;
    while (isRunning_) {
        getSerialData();
        // 每1000次循环（约10秒）打印一次状态，确认线程还在运行
        if (++loopCount % 1000 == 0) {
            LOG_INFO("串口接收线程运行中... (已循环 %d 次)", loopCount);
        }
    }
    LOG_INFO("串口接收线程已退出");
}

void CTurnOnRobot::closeUsart()
{
    // 发送停止命令
    sendSerialData_[0]  = FRAME_HEADER;
    sendSerialData_[1]  = 0;
    sendSerialData_[2]  = 0;
    sendSerialData_[3]  = 0;
    sendSerialData_[4]  = 0;
    sendSerialData_[5]  = 0;
    sendSerialData_[6]  = 0;
    sendSerialData_[7]  = 0;
    sendSerialData_[8]  = 0;
    sendSerialData_[9]  = checkSum(9, SEND_DATA_CHECK);
    sendSerialData_[10] = FRAME_TAIL;

    try {
        if (mcuSerial_ && mcuSerial_->is_open()) {
            std::vector<uint8_t> sendData(sendSerialData_, sendSerialData_ + sizeof(sendSerialData_));
            mcuSerial_->send(sendData);
            mcuSerial_->close();
            LOG_INFO("串口已关闭");
        }
    } catch (const std::exception& e) {
        LOG_ERROR("关闭串口时发送数据失败: %s", e.what());
    }
    return;
}

bool CTurnOnRobot::isUsartOpen()
{
    return mcuSerial_ && mcuSerial_->is_open();
}

void CTurnOnRobot::getEnableRobot(uint8_t& enableRobot)
{
    enableRobot = enableRobot_;
}

void CTurnOnRobot::getOdomData(SOdometryData& odomData)
{
    std::lock_guard<std::mutex> lock(serialMutex_);
    odomData = odomData_;
    return;
}

void CTurnOnRobot::getImuData(SImuData& imuData)
{
    std::lock_guard<std::mutex> lock(serialMutex_);
    imuData = imuData_;
    return;
}

void CTurnOnRobot::getRobotVoltage(float& powerVoltage)
{
    std::lock_guard<std::mutex> lock(serialMutex_);
    powerVoltage = powerVoltage_;
    return;
}

/**
 * @brief 将两个字节转换为有符号短整型 (Short)
 * @param dataHigh 高字节
 * @param dataLow 低字节
 * @return 转换后的有符号短整型值
 * 
 * 说明：
 * - C++ 的 short 类型使用补码表示负数
 * - 当 dataHigh 的最高位（bit 7）为 1 时，表示负数（补码形式）
 * - C++ 编译器会自动将补码转换为正确的负数，无需手动转换
 * - 例如：
 *   - 正数：0x0064 (100) -> short = 100
 *   - 负数：0xFF9C (-100的补码) -> short = -100（自动转换）
 * 
 * 根据文档：
 * - STM32 发送正数时为原码（原码 = 补码）
 * - STM32 发送负数时为补码
 * - 直接使用 short 类型读取即可，编译器会自动处理
 */
short CTurnOnRobot::imuTrans(const uint8_t dataHigh, const uint8_t dataLow)
{
    short temp{0};
    temp |= static_cast<short>(dataHigh) << 8;
    temp |= static_cast<short>(dataLow);
    // 注意：如果 dataHigh 的最高位是 1，temp 会被自动解释为负数（补码）
    // 这是 C++ short 类型的标准行为，无需额外处理
    return temp;
}

/**
 * @brief 将速度数据从 mm/s 转换为 m/s
 * @param dataHigh 高字节
 * @param dataLow 低字节
 * @return 转换后的速度值 (m/s)
 */
float CTurnOnRobot::odomTrans(const uint8_t dataHigh, const uint8_t dataLow)
{
    short temp = imuTrans(dataHigh, dataLow);
    // 单位 mm/s，需要除以 1000 转换为 m/s
    return static_cast<float>(temp) / 1000.0f;
}

unsigned char CTurnOnRobot::checkSum(const unsigned char countNumber, const unsigned char mode)
{
    unsigned char check_sum{0};

    if (mode == READ_DATA_CHECK) {
        for (size_t i{0}; i < countNumber; i++) {
            check_sum ^= receivedSerialData_[i];
        }
    }
    if (mode == SEND_DATA_CHECK) {
        for (size_t i{0}; i < countNumber; i++) {
            check_sum ^= sendSerialData_[i];
        }
    }

    return check_sum;
}

bool CTurnOnRobot::getSerialData()
{
    if (!mcuSerial_ || !mcuSerial_->is_open()) {
        return false;
    }

    uint8_t buffer[RECEIVE_DATA_SIZE] = {0};
    size_t  bytesRead{0};
    
    static int noDataCount = 0;      // 连续无数据计数
    static int nonHeaderCount = 0;   // 连续非帧头字节计数
    // 增大缓冲区，至少能容纳256字节的数据（足够处理多次读取）
    static constexpr int DATA_BUFFER_SIZE = 256;
    static uint8_t dataBuffer[DATA_BUFFER_SIZE] = {0};  // 数据缓冲区，用于缓存接收到的数据
    static int bufferLength = 0;     // 缓冲区中有效数据长度
    
    while (isRunning_) {
        // 读取可用数据（可能一次读取多个字节）
        std::vector<uint8_t> tempBuffer(256);  // 临时缓冲区，足够大以接收多个字节
        try {
            bytesRead = mcuSerial_->receive(tempBuffer);
        } catch (const std::exception& e) {
            LOG_ERROR("无法从串口读取数据: %s", e.what());
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }

        if (bytesRead == 0) {
            noDataCount++;
            // 每5秒打印一次（500次 * 10ms）
            if (noDataCount % 500 == 0) {
                LOG_INFO("等待串口数据... (已等待 %.1f 秒)", noDataCount * 0.01);
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
        
        noDataCount = 0; // 收到数据，重置计数

        // 将接收到的数据添加到缓冲区
        const int bufferSize = DATA_BUFFER_SIZE;
        if (bufferLength + bytesRead > bufferSize) {
            // 缓冲区溢出，需要处理
            LOG_ERROR("接收缓冲区溢出！bufferLength=%d, bytesRead=%zu, 最大容量=%d", 
                      bufferLength, bytesRead, bufferSize);
            
            if (bytesRead >= bufferSize) {
                // 新数据本身就超过了缓冲区大小，只保留新数据的最后部分
                LOG_ERROR("新数据(%zu字节)超过缓冲区大小(%d字节)，只保留最后%d字节", 
                          bytesRead, bufferSize, bufferSize);
                int offset = bytesRead - bufferSize;
                memcpy(dataBuffer, tempBuffer.data() + offset, bufferSize);
                bufferLength = bufferSize;
            } else {
                // 新数据加上旧数据超过缓冲区，保留旧数据的最后部分和新数据
                int keepLength = bufferSize - bytesRead;
                if (keepLength > 0) {
                    int srcStart = bufferLength - keepLength;
                    // 检查边界，防止越界
                    if (srcStart >= 0 && srcStart + keepLength <= bufferSize) {
                        memmove(dataBuffer, dataBuffer + srcStart, keepLength);
                        memcpy(dataBuffer + keepLength, tempBuffer.data(), bytesRead);
                        bufferLength = bufferSize;
                    } else {
                        LOG_ERROR("缓冲区溢出处理时越界！srcStart=%d, keepLength=%d, bufferSize=%d", 
                                  srcStart, keepLength, bufferSize);
                        // 如果旧数据处理有问题，直接使用新数据
                        int copyLen = (bytesRead < bufferSize) ? bytesRead : bufferSize;
                        memcpy(dataBuffer, tempBuffer.data() + (bytesRead - copyLen), copyLen);
                        bufferLength = copyLen;
                    }
                } else {
                    // keepLength <= 0，说明新数据本身就占满了整个缓冲区
                    memcpy(dataBuffer, tempBuffer.data() + (bytesRead - bufferSize), bufferSize);
                    bufferLength = bufferSize;
                }
            }
        } else {
            // 正常情况，直接添加数据
            memcpy(dataBuffer + bufferLength, tempBuffer.data(), bytesRead);
            bufferLength += bytesRead;
        }

        // 在缓冲区中查找完整的数据帧
        while (bufferLength >= RECEIVE_DATA_SIZE) {
            // 查找帧头
            int frameStartIndex = -1;
            for (int i = 0; i <= bufferLength - RECEIVE_DATA_SIZE; i++) {
                if (dataBuffer[i] == FRAME_HEADER) {
                    frameStartIndex = i;
                    break;
                }
            }

            if (frameStartIndex == -1) {
                // 未找到帧头，清除缓冲区中除了最后几个字节外的所有数据
                // 保留最后 RECEIVE_DATA_SIZE - 1 个字节，以防帧头被分割
                int keepLength = RECEIVE_DATA_SIZE - 1;
                if (bufferLength > keepLength) {
                    int srcStart = bufferLength - keepLength;
                    // 检查边界，防止越界
                    if (srcStart >= 0 && srcStart + keepLength <= DATA_BUFFER_SIZE) {
                        memmove(dataBuffer, dataBuffer + srcStart, keepLength);
                        bufferLength = keepLength;
                    } else {
                        LOG_ERROR("未找到帧头时缓冲区操作越界！srcStart=%d, keepLength=%d, bufferSize=%d", 
                                  srcStart, keepLength, DATA_BUFFER_SIZE);
                        bufferLength = 0;
                    }
                }
                nonHeaderCount++;
                // 每收到100个非帧头字节时打印一次，便于调试数据流
                if (nonHeaderCount % 100 == 0) {
                    std::printf("[DEBUG] 未找到帧头，已收到 %d 个非帧头字节\n", nonHeaderCount);
                }
                break;
            }

            nonHeaderCount = 0; // 找到帧头，重置计数

            // 检查是否有足够的数据（从帧头开始至少24字节）
            if (frameStartIndex + RECEIVE_DATA_SIZE > bufferLength) {
                // 数据不完整，等待更多数据
                break;
            }

            // 提取完整数据帧
            memcpy(buffer, dataBuffer + frameStartIndex, RECEIVE_DATA_SIZE);

            // 验证帧头和帧尾
            unsigned char frameHeader = buffer[0];
            unsigned char frameTail   = buffer[RECEIVE_DATA_SIZE - 1];


            if (frameHeader != FRAME_HEADER || frameTail != FRAME_TAIL) {
                // 帧头或帧尾不匹配，从下一个位置继续查找
                LOG_ERROR("帧头或帧尾不匹配！期望帧头=0x%02X, 实际=0x%02X; 期望帧尾=0x%02X, 实际=0x%02X", 
                          FRAME_HEADER, frameHeader, FRAME_TAIL, frameTail);
                std::printf("[ERROR] 收到的数据: ");
                for (int i = 0; i < RECEIVE_DATA_SIZE; ++i) {
                    std::printf("%02X ", buffer[i]);
                }
                std::printf("\n");
                // 计算剩余数据的起始位置和长度
                int remainingStart = frameStartIndex + 1;
                int remainingLength = bufferLength - remainingStart;
                
                // 检查边界，防止越界
                if (remainingStart >= 0 && remainingLength >= 0 && 
                    remainingStart < DATA_BUFFER_SIZE &&
                    remainingStart + remainingLength <= DATA_BUFFER_SIZE) {
                    // 更新缓冲区长度
                    bufferLength = remainingLength;
                    
                    // 如果有剩余数据，将其移动到缓冲区开头
                    if (remainingLength > 0) {
                        memmove(dataBuffer, dataBuffer + remainingStart, remainingLength);
                    }
                } else {
                    LOG_ERROR("缓冲区操作越界！frameStartIndex=%d, remainingStart=%d, remainingLength=%d, bufferLength=%d, bufferSize=%d", 
                              frameStartIndex, remainingStart, remainingLength, bufferLength, DATA_BUFFER_SIZE);
                    bufferLength = 0;  // 重置缓冲区，防止进一步错误
                }
                continue;
            }

            // 复制数据到接收缓冲区
            memcpy(receivedSerialData_, buffer, RECEIVE_DATA_SIZE);

            // 校验和验证（前22字节异或校验）
            bool verify = receivedSerialData_[22] == checkSum(22, READ_DATA_CHECK);

            // 从缓冲区中移除已处理的数据
            // 计算剩余数据的起始位置和长度
            int remainingStart = frameStartIndex + RECEIVE_DATA_SIZE;
            int remainingLength = bufferLength - remainingStart;
            
            // 检查边界，防止越界
            if (remainingStart >= 0 && remainingLength >= 0 && 
                remainingStart < static_cast<int>(sizeof(dataBuffer)) &&
                remainingStart + remainingLength <= static_cast<int>(sizeof(dataBuffer))) {
                // 更新缓冲区长度
                bufferLength = remainingLength;
                
                // 如果有剩余数据，将其移动到缓冲区开头
                if (remainingLength > 0) {
                    memmove(dataBuffer, dataBuffer + remainingStart, remainingLength);
                }
            } else {
                LOG_ERROR("缓冲区操作越界！frameStartIndex=%d, remainingStart=%d, remainingLength=%d, bufferLength=%d, bufferSize=%zu", 
                          frameStartIndex, remainingStart, remainingLength, bufferLength, sizeof(dataBuffer));
                bufferLength = 0;  // 重置缓冲区，防止进一步错误
            }

            // 打印接收到的完整数据帧（原始十六进制）
            // std::printf("[RECV] 收到一帧串口数据 (原始): ");
            // for (int i = 0; i < RECEIVE_DATA_SIZE; ++i) {
            //     std::printf("%02X ", receivedSerialData_[i]);
            // }
            // std::printf("\n");

            if (verify) {
                std::lock_guard<std::mutex> lock(serialMutex_);

                // 使能状态
                enableRobot_ = receivedSerialData_[1];

                // 里程计数据 (速度)
                odomData_.timestamp_ = rclcpp::Clock().now();
                // X轴速度：单位 mm/s，转换为 m/s
                odomData_.vxData_ = odomTrans(receivedSerialData_[2], receivedSerialData_[3]);
                // Y轴速度：单位 mm/s，转换为 m/s
                odomData_.vyData_ = odomTrans(receivedSerialData_[4], receivedSerialData_[5]);
                // Z轴速度：单位 0.01 rad/s（放大100倍），转换为 rad/s
                odomData_.vzData_ = static_cast<float>(imuTrans(receivedSerialData_[6], receivedSerialData_[7])) / 100.0f;

                // IMU 数据
                imuData_.timestamp_ = rclcpp::Clock().now();
                // 加速度计数据：需要除以 1672 得到 m/s^2
                imuData_.linearAccelerationXData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[8], receivedSerialData_[9])) / ACCEL_SCALE;
                imuData_.linearAccelerationYData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[10], receivedSerialData_[11])) / ACCEL_SCALE;
                imuData_.linearAccelerationZData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[12], receivedSerialData_[13])) / ACCEL_SCALE;
                // 陀螺仪数据：需要除以 3753 得到 rad/s
                imuData_.angularVelocityXData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[14], receivedSerialData_[15])) / GYROSCOPE_SCALE;
                imuData_.angularVelocityYData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[16], receivedSerialData_[17])) / GYROSCOPE_SCALE;
                imuData_.angularVelocityZData_ =
                    static_cast<double>(imuTrans(receivedSerialData_[18], receivedSerialData_[19])) / GYROSCOPE_SCALE;

                // 电池电压：单位 mv，除以 1000 得到 V
                // 使用有符号 Short 类型解析，最高位为符号位
                powerVoltage_ = static_cast<float>(imuTrans(receivedSerialData_[20], receivedSerialData_[21])) / 1000.0f;

                // 打印成功接收并解析的数据（按数据传输顺序）
                // LOG_INFO("✓ 数据帧接收成功并校验通过 - 使能状态:%d, "
                //          "速度(vx:%.3f m/s, vy:%.3f m/s, vz:%.3f rad/s), "
                //          "加速度(ax:%.3f m/s², ay:%.3f m/s², az:%.3f m/s²), "
                //          "角速度(gx:%.3f rad/s, gy:%.3f rad/s, gz:%.3f rad/s), "
                //          "电压:%.2fV", 
                //          enableRobot_,
                //          odomData_.vxData_, odomData_.vyData_, odomData_.vzData_,
                //          imuData_.linearAccelerationXData_, imuData_.linearAccelerationYData_, imuData_.linearAccelerationZData_,
                //          imuData_.angularVelocityXData_, imuData_.angularVelocityYData_, imuData_.angularVelocityZData_,
                //          powerVoltage_);
            } else {
                // 校验失败，打印错误和数据内容
                unsigned char expectedChecksum = checkSum(22, READ_DATA_CHECK);
                unsigned char receivedChecksum = receivedSerialData_[22];
                LOG_ERROR("✗ 数据帧校验失败！期望校验和=0x%02X, 实际校验和=0x%02X", 
                          expectedChecksum, receivedChecksum);
                std::printf("[ERROR] 收到的数据: ");
                for (int i = 0; i < RECEIVE_DATA_SIZE; ++i) {
                    std::printf("%02X ", receivedSerialData_[i]);
                }
                std::printf("\n");
                continue;
            }
        }
    }
    return false;
}

void CTurnOnRobot::setRobotVel(const SVelocityData& setRobotVel)
{
    if (!mcuSerial_ || !mcuSerial_->is_open()) {
        return;
    }

    short temp;
    sendSerialData_[0] = FRAME_HEADER;  // 帧头
    sendSerialData_[1] = 0;             // 预留位
    sendSerialData_[2] = 0;             // 预留位

    // 机器人 X 轴目标速度
    temp               = 0;
    temp               = static_cast<short>(setRobotVel.x_ * 1000);
    sendSerialData_[4] = temp & 0xFF;
    sendSerialData_[3] = (temp >> 8) & 0xFF;

    // 机器人 Y 轴目标速度
    temp               = 0;
    temp               = static_cast<short>(setRobotVel.y_ * 1000);
    sendSerialData_[6] = temp & 0xFF;
    sendSerialData_[5] = (temp >> 8) & 0xFF;

    // 机器人角速度
    temp               = 0;
    temp               = static_cast<short>(setRobotVel.w_ * 1000);
    sendSerialData_[8] = temp & 0xFF;
    sendSerialData_[7] = (temp >> 8) & 0xFF;

    // 校验位
    sendSerialData_[9]  = checkSum(9, SEND_DATA_CHECK);
    sendSerialData_[10] = FRAME_TAIL;

    try {
        std::vector<uint8_t> sendData(sendSerialData_, sendSerialData_ + sizeof(sendSerialData_));
        mcuSerial_->send(sendData);
    } catch (const std::exception& e) {
        LOG_ERROR("速度发送失败: %s", e.what());
    }
}
